import math

from rtde_receive import RTDEReceiveInterface
import time

from utils.z_theta_and_Rxyz_new import Rxyz_to_theta

rtde_r = RTDEReceiveInterface("192.168.111.130")

def test1():
    while True:
        q_actual = rtde_r.getActualQ()  # 六个关节角 (rad)
        tcp_pose = rtde_r.getActualTCPPose()  # TCP 位姿 (x,y,z,Rx,Ry,Rz)
        print("Joint angles:", q_actual)
        print("TCP pose:", tcp_pose)
        time.sleep(0.1)

def test2():
    while True:
        tcp_pose = rtde_r.getActualTCPPose()
        print(tcp_pose[3:])
        print(
            "z_angle: {:.2f}".format(
                Rxyz_to_theta(tcp_pose[3], tcp_pose[4], tcp_pose[5]) / math.pi * 180,
            )
        )
        time.sleep(0.5)

if __name__ == '__main__':
    test2()